//------------------------------------------------------------------------------
// Referee for Sim Magellan Service
//
// Professional Microsoft Robotics Developer Studio
//
// Updated for RDS 2008 (V2.0)
//
//------------------------------------------------------------------------------
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;

using System;
using System.Collections.Generic;
using W3C.Soap;

using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using engineproxy = Microsoft.Robotics.Simulation.Engine.Proxy;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;
using System.ComponentModel;

using xna = Microsoft.Xna.Framework;
using xnagrfx = Microsoft.Xna.Framework.Graphics;

using corobot = ProMRDS.Simulation.Corobot;

namespace ProMRDS.Simulation.MagellanReferee
{
    [DisplayName("Simulated Magellan Referee")]
    [Description("A referee service for the Magellan simulation.")]
    [Contract(Contract.Identifier)]
    public class MagellanReferee : DsspServiceBase
    {
        public Barrier[] Barriers = new Barrier[]
        {
            new Barrier("Wall0", new Vector3(0, 0, -4), new Vector3(4f, 0.8f, 0.1f), "BrickWall.dds", new Quaternion(0,0,0,1)),
            new Barrier("Wall1", new Vector3(-2.05f, 0, -3.05f), new Vector3(2f, 0.8f, 0.1f), "BrickWall.dds", Quaternion.FromAxisAngle(0, 1, 0, (float)(Math.PI / 2))),
            new Barrier("Wall2", new Vector3(1.41f, 0, 2.46f), new Vector3(6f, 0.8f, 0.1f), "BrickWall.dds", Quaternion.FromAxisAngle(0, 1, 0, (float)(Math.PI / 2))),
            new Barrier("Tower", new Vector3(5.58f, 2f, -0.59f), new Vector3(2f, 4f, 2f), "MayangConcrP.dds", new Quaternion(0,0,0,1)),
        };

        [Partner("Engine", 
            Contract = engineproxy.Contract.Identifier, 
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        private engineproxy.SimulationEnginePort _engineStub = 
            new engineproxy.SimulationEnginePort();

        [InitialStatePartner(Optional = true, ServiceUri = "Magellan.Referee.config.xml")]
        private MagellanRefereeState _state = new MagellanRefereeState();

        // Main service port
        [ServicePort("/MagellanReferee", AllowMultipleInstances=false)]
        private MagellanRefereeOperations _mainPort = 
            new MagellanRefereeOperations();

        public MagellanReferee(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {

        }

        protected override void Start()
        {
            // make sure the state is valid
            ValidateState();

            base.Start();

            // Add objects (entities) in our simulated world
            PopulateWorld();
        }

        private void ValidateState()
        {
            if (_state == null)
                _state = new MagellanRefereeState();

            if ((_state.Cones == null) || (_state.Cones.Length == 0))
            {
                // default state
                _state.Cones = new Vector3[]
                {
                    new Vector3(0, 0, -5),
                    new Vector3(10, 0, 4),
                    new Vector3(-3, 0, 2)
                };
                base.SaveState(_state);
            }
        }

        private void PopulateWorld()
        {
            // Set up initial view
            CameraView view = new CameraView();
            view.EyePosition = new Vector3(-0.91f, 0.67f, -1f);
            view.LookAtPoint = new Vector3(1.02f, 0.09f, 0.19f);
            SimulationEngine.GlobalInstancePort.Update(view);

            // create a very simple world and add a Corobot to it
            // Add another camera to view the scene from above
            CameraEntity fromAbove = new CameraEntity(640, 480);
            fromAbove.State.Name = "FromAbove";
            fromAbove.Location = new xna.Vector3(4.3f, 20.59f, 0.86f);
            fromAbove.LookAt = new xna.Vector3(4.29f, 18.26f, 0.68f);
            fromAbove.IsRealTimeCamera = false;
            SimulationEngine.GlobalInstancePort.Insert(fromAbove);

            // Add a SkyDome.
            SkyDomeEntity sky = new SkyDomeEntity("skydome.dds", "sky_diff.dds");
            SimulationEngine.GlobalInstancePort.Insert(sky);

            // Add a directional light to simulate the sun.
            LightSourceEntity sun = new LightSourceEntity();
            sun.State.Name = "Sun";
            sun.Type = LightSourceEntityType.Directional;
            sun.Color = new Vector4(1, 1, 1, 1);
            sun.Direction = new Vector3(-0.47f, -0.8f, -0.36f);
            SimulationEngine.GlobalInstancePort.Insert(sun);

            HeightFieldShapeProperties hf = new HeightFieldShapeProperties("height field",
                64, // number of rows 
                10, // distance in meters, between rows
                64, // number of columns
                10, // distance in meters, between columns
                1, // scale factor to multiple height values 
                -1000); // vertical extent of the height field. Should be set to large negative values

            // create array with height samples
            hf.HeightSamples = new HeightFieldSample[hf.RowCount * hf.ColumnCount];
            for (int i = 0; i < hf.RowCount * hf.ColumnCount; i++)
            {
                hf.HeightSamples[i] = new HeightFieldSample();
                hf.HeightSamples[i].Height = (short)(Math.Sin(i * 0.01));
            }

            // create a material for the entire field. We could also specify material per sample.
            hf.Material = new MaterialProperties("ground", 0.8f, 0.5f, 0.8f);

            // insert ground entity in simulation and specify a texture
            SimulationEngine.GlobalInstancePort.Insert(new HeightFieldEntity(hf, "FieldGrass.dds"));

            // create barriers
            foreach (Barrier bar in Barriers)
            {
                SingleShapeEntity wall =
                    new SingleShapeEntity(
                        new BoxShape(
                            new BoxShapeProperties(
                                0,  // no mass makes a static shape
                                new Pose(),
                                bar.Dimensions)), // dimensions
                            bar.Position);
                wall.State.Pose.Orientation = bar.Orientation;
                wall.State.Name = bar.Name;
                wall.State.Assets.DefaultTexture = bar.Texture;
                SimulationEngine.GlobalInstancePort.Insert(wall);
            }

            // create cones
            for (int coneCount = 0; coneCount < _state.Cones.Length; coneCount++)
                SimulationEngine.GlobalInstancePort.Insert(
                    CreateTriggerCone("Cone" + coneCount.ToString(), _state.Cones[coneCount]));

            // create a Corobot
            SimulationEngine.GlobalInstancePort.Insert(new corobot.CorobotEntity("Corobot", new Vector3(0, 0, 0)));
        }

        private VisualEntity CreateTriggerCone(
            string name,
            Vector3 position)
        {
            Vector3 dimensions = new Vector3(0.35f, 0.7f, 0.35f);
            position.Y += dimensions.Y / 2f;
            SingleShapeEntity triggerShape = new SingleShapeEntity(
                new BoxShape(
                    new BoxShapeProperties(
                        0,
                        new Pose(),
                        dimensions)),
                position);

            // used to receive a single trigger notification from this cone
            Port<Shape> conePort = new Port<Shape>();


            triggerShape.State.Name = name;
            triggerShape.State.Assets.Mesh = "street_cone.obj";
            triggerShape.BoxShape.State.Name = name;
            triggerShape.BoxShape.State.Advanced = new ShapeAdvancedProperties();
            // make the box a trigger shape by adding a notification port
            triggerShape.BoxShape.State.Advanced.IsTrigger = true;
            triggerShape.BoxShape.State.Advanced.TriggerNotificationPort = conePort;
            triggerShape.State.MassDensity.Mass = 0;
            triggerShape.State.MassDensity.Density = 0;

            Activate(Arbiter.Receive<Shape>(false, conePort,
                delegate(Shape sh)
                {
                    Console.WriteLine("Cone " + name + " was triggered.");
                }
            ));

            return triggerShape;
        }

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }
    }

    public struct Barrier
    {
        public string Name;
        public Vector3 Position;
        public Vector3 Dimensions;
        public string Texture;
        public Quaternion Orientation;
        public Barrier(string name, Vector3 position, Vector3 dimensions, string texture, Quaternion orientation)
        {
            Name = name;
            Position = position;
            Dimensions = dimensions;
            Texture = texture;
            Orientation = orientation;
        }
    }

    public static class Contract
    {
        public const string Identifier = "http://www.promrds.com/contracts/2007/08/magellanreferee.html";
    }

    #region State
    /// <summary>
    /// Magellan Referee State
    /// </summary>
    [DataContract()]
    public class MagellanRefereeState
    {
        [DataMember]
        public Vector3[] Cones;
    }
    #endregion

    [ServicePort]
    public class MagellanRefereeOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop, Get>
    {
    }
    
    public class Get : Get<GetRequestType, PortSet<MagellanRefereeState, Fault>>
    {
    }

}
